#pragma once
#include "vio/front_end/camera.h"
#include "vio/front_end/feature.h"
#include "vio/front_end/feature_detection.h"
#include "vio/front_end/frame.h"
#include "vio/front_end/map.h"
#include "vio/front_end/point.h"

namespace vio {
class VIOPipeline {
 public:
  VIOPipeline(){};
  // API
  void Initialize();
  void Step();
  void Terminate();
  // msg callbacks
  void IMUCallback(const double& ts,
                   const Eigen::Vector3d& accel,
                   const Eigen::Vector3d& gyro);
  void LeftImgCallback(const double& ts, const cv::Mat& img);
  void RightImgCallback(const double& ts, const cv::Mat& img);

  const Camera::Ptr GetCam() const {
    return camera_ptr_;
  }

 protected:
  void DrawKeyPoints(cv::Mat& img, vio::Features& fts);

 private:
  Camera::Ptr camera_ptr_{nullptr};
  Frame::Ptr curr_frame_ptr_{nullptr};
  Frame::Ptr prev_frame_ptr_{nullptr};
  feature_detection::FastDetector::Ptr fast_detector_ptr_{nullptr};
};
}  // namespace vio
